/*********************************************************************
*
*   RobotBase.cpp - Robot base class
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/

#include <stdio.h>
#include <math.h>

#include "RobotBase.h"
#include "RobotMath.h"

static FILE * s_debugFile;
extern RobotBase *  robot_ptr;

RobotBase * RobotBase::p_robot = NULL;

void RobotBase::Update( )
{
// Call student code
StudentPeriodic( );

// Re-compute robot position every 100ms
m_ticks += 5;
if( ( m_ticks % 100 ) == 0 )
    {
    MoveRobot( );
    //fprintf( m_logFile,
    //         "%f, %f, %f, %f, %f, %f\n",
    //       m_ticks / 1000.0, m_x, m_y, m_yaw, m_leftMotor, m_rightMotor );
    }
}


RobotBase::RobotBase( )
{
    m_leftMotor = 0.0;
    m_rightMotor = 0.0;
    m_x = 0.0;  //CDL new starting position here
    m_y = 0.0;
    m_yaw = 0.0;
    m_width = 10;
    m_ticks = 0;
    leftMotorAccumulator = 0.0;
    rightMotorAccumulator = 0.0;

    //m_logFile = fopen( "robotLog.csv", "w" );
    //fprintf( m_logFile, "time, x, y, yaw, left motor, right motor\n" );

    //s_debugFile = fopen( "debugLog.csv", "w" );
    //fprintf( s_debugFile, "rotation angle, rotation origin,,rotation arm, rotation arm check\n" );

    robot_ptr = this;
    RobotBase::p_robot = this;
}


RobotBase::~RobotBase( )
{
    robot_ptr = NULL;
    //fclose( m_logFile );
    //fclose( s_debugFile );
}


void RobotBase::SetPos( double x, double y )
{
m_x = x;
m_y = y;
}


void RobotBase::SetAngle( double a )
{
m_yaw = ( a * ( PI / 180.0 ) );
}


void RobotBase::MoveRobot( )
{
    // Scale motor speed so robot does not move so fast
    double leftMotor  = m_leftMotor / MOTOR_SCALE;
    double rightMotor = m_rightMotor / MOTOR_SCALE;
    leftMotorAccumulator += leftMotor;
    rightMotorAccumulator += rightMotor;

    // If both wheels are turning at the same speed, then move the robot
    // straight
    if( fabs( leftMotor - rightMotor ) < WHEEL_SPEED_EPSILON )
        {
        double theta = ConvertYawToTheta( m_yaw );
        double dx = leftMotor * cos( theta );
        double dy = leftMotor * sin( theta );
        m_x = m_x + dx;
        m_y = m_y + dy;
        }

    // If wheels are turning at different speeds, then the robot will turn
    else
        {
        double rotationAngle;
        double rotationDistance;
        ComputeRotationAngle( leftMotor,
                              rightMotor,
                              m_width,
                              &rotationAngle,
                              &rotationDistance );

        // Determine rotation axis coordinates by rotating
        // (rotationDistance,0) vector around the robot position through
        // the yaw angle
        double xo = m_x + rotationDistance;
        double yo = m_y;
        RotateAboutPoint( &xo, &yo, m_x, m_y, m_yaw );

        //fprintf( s_debugFile,
        //         "%f, %f, %f, %f, %f\n",
        //       rotationAngle * 180.0 / PI,
        //       xo,
        //       yo,
        //       rotationDistance,
        //       sqrt( ( m_x - xo ) * ( m_x - xo ) + ( m_y - yo ) * ( m_y - yo ) ) );
        RotateAboutPoint( &m_x, &m_y, xo, yo, rotationAngle );
        m_yaw = fmod( m_yaw + rotationAngle, 2 * PI );
        }
}


void RobotBase::SetMotor( double * p_motor, double speed )
{
    if( speed < -1.0 )
    {
        *p_motor = -1.0;
    }
    else if( speed > 1.0 )
    {
        *p_motor = 1.0;
    }
    else
    {
        *p_motor = speed;
    }
}


/******************************************************************************
* Cycle through different motor settings
******************************************************************************/
void RobotBase::TestPeriodic( unsigned int t_ms )
{
if( ( t_ms % 1000 ) == 0 )
    {
    m_rightMotor += 0.5;
    if( m_rightMotor > 1.1 )
        {
        m_rightMotor = -1.0;
        m_leftMotor += 0.5;
        if( m_leftMotor > 1.1 )
            {
            m_leftMotor = -1.0;
            }
        }
    }
}
